#include "geometricutil.h"
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <QtMath>
#include <QStringList>
#include <QDebug>
#include "commonutil.h"
#include "shipAlgorithm/Mercator.h"

using namespace ns_yht;

namespace bg = boost::geometry;
typedef boost::geometry::model::d2::point_xy<double> BPointF;

const double GeometricUtil::MYPI = 3.14159265359;    // PI
const double GeometricUtil::PI2 = 6.28318530718;     // PI*2
const double GeometricUtil::PID2 = 1.570796326795;   // PI/2
const double GeometricUtil::PI3D2 = 4.712388980385;  // PI*3/2
const double GeometricUtil::DPPi = 57.295779513;     // 180/PI
const double GeometricUtil::PiPD = 0.01745329252;    // PI/180
const double GeometricUtil::EARTH_RADIUS = 6378.137; // 地球半径，单位：公里

GeometricUtil::GeometricUtil()
{
}

void GeometricUtil::simplifyLine(const QVector<QPointF> &spoints, QVector<QPointF> &dpoints, double dis)
{
    typedef boost::geometry::model::d2::point_xy<double> xy;

    boost::geometry::model::linestring<xy> line;
    for (int i = 0; i < spoints.count(); i++)
        line.push_back(xy(spoints[i].x(), spoints[i].y()));

    // Simplify it, using distance of 0.5 units
    boost::geometry::model::linestring<xy> simplified;
    boost::geometry::simplify(line, simplified, dis);

    for (boost::geometry::model::linestring<xy>::iterator it = simplified.begin(); it != simplified.end(); it++)
    {
        dpoints.append(QPointF(it->x(), it->y()));
    }
}

// 正规化一个矩形，确保第二个点的XY值均大于等于第一个点的XY值。
QRect GeometricUtil::normalizeRectangle(double x1, double y1, double x2, double y2)
{
    if (x2 < x1)
    {
        int tmp = x2;
        x2 = x1;
        x1 = tmp;
    }

    if (y2 < y1)
    {
        int tmp = y2;
        y2 = y1;
        y1 = tmp;
    }
    return QRect(x1, y1, x2 - x1, y2 - y1);
}

QRect GeometricUtil::normalizeRectangle(const QPointF &pt1, const QPointF &pt2)
{
    return normalizeRectangle(pt1.x(), pt1.y(), pt2.x(), pt2.y());
}

// 返回点p以点o为圆心逆时针旋转alpha(单位：度)后所在的位置
QPointF GeometricUtil::rotateOnePoint(const QPointF &o, double alpha, const QPointF &p)
{
    QPointF tp;
    QPointF pp = p;
    pp.setX(p.x() - o.x());
    pp.setY(p.y() - o.y());
    double al = alpha * PiPD;
    tp.setX(pp.x() * cos(al) - pp.y() * sin(al) + o.x());
    tp.setY(pp.y() * cos(al) + pp.x() * sin(al) + o.y());
    return tp;
}

/// <summary>
/// 坐标系以(pt0.x,pt0.y)为中心，水平向右的方向为X轴，顺时针方向为角的方向获得(pt1.x,pt1.y)在该坐标系中的角度
/// </summary>
/// <param name="pt0">起始点</param>
/// <param name="pt1">终止点</param>
/// <returns>(0, 360)之间的角度值</returns>
double GeometricUtil::getAngle(const QPointF &pt0, const QPointF &pt1)
{
    if (qAbs(pt0.x() - pt1.x()) < 1e-6 && qAbs(pt0.y() - pt1.y()) < 1e-6)
        return 0; // 同一个点
    if (qAbs(pt0.x() - pt1.x()) < 1e-6)
        return pt0.y() > pt1.y() ? 270.0 : 90.0; // 竖直
    if (qAbs(pt0.y() - pt1.y()) < 1e-6)
        return pt0.x() > pt1.x() ? 180.0 : 0.0; // 水平

    double a = qRadiansToDegrees(qAtan2(pt1.y() - pt0.y(), pt1.x() - pt0.x()));
    if (a > 360)
        a -= 360;
    else if (a < 0)
        a += 360;
    return a;

    //    double fTmp = qAtan(qAbs((pt0.y() - pt1.y()) / (pt0.x() - pt1.x())));

    //    if (pt0.x() > pt1.x()) fTmp = pt0.y() > pt1.y() ? (MYPI + fTmp) : (MYPI - fTmp);
    //    else fTmp = pt0.y() > pt1.y() ? (PI2 - fTmp) : fTmp;
    //    return fTmp * DPPi;		//弧度值转化为角度值
}

double GeometricUtil::getAngleNorth(const QPointF &pt0, const QPointF &pt1)
{
    double angle = getAngle(pt0, pt1);
    angle += 90;
    if (angle >= 360)
        angle -= 360;
    return angle;
}

/// <summary>
/// 以ptBase为起点，在方向angle上取长度为distance所得到的点
/// </summary>
QPointF GeometricUtil::getPtInAngle(QPointF &ptBase, double angle, double distance)
{
    QPointF pt(ptBase.x() + distance, ptBase.y());
    return rotateOnePoint(ptBase, angle, pt);
}

/// <summary>
/// 把角度值正规化，不是弧度值
/// normalizedType=1 —— 返回[-180,180)之间的角度值
/// normalizedType=2 —— 返回[0,360)之间的角度值
/// </summary>
double GeometricUtil::getNormalizedAngle(double angle, int normalizedType)
{
    int n = (int)(angle / 360);
    if (angle < 0)
        n -= 1;
    angle = angle - n * 360;
    if (normalizedType == 1)
        angle -= 180;
    return angle;
}

/// <summary>
/// 获取o,s 与 o,e 两条直线构成的夹角,在-180，180之间
/// </summary>
double GeometricUtil::getTwoLinesAngle(const QPointF &o, const QPointF &s, const QPointF &e)
{
    double cosfi, fi, norm;
    double dsx = s.x() - o.x();
    double dsy = s.y() - o.y();
    double dex = e.x() - o.x();
    double dey = e.y() - o.y();

    cosfi = dsx * dex + dsy * dey;
    norm = (dsx * dsx + dsy * dsy) * (dex * dex + dey * dey);

    if (norm < 1)
        return 0; // norm接近0会有问题

    cosfi /= sqrt(norm);

    if (cosfi >= 1.0)
        return 0;
    if (cosfi <= -1.0)
        return -MYPI * DPPi;

    fi = acos(cosfi);
    if (dsx * dey - dsy * dex > 0)
        return fi * DPPi; // 说明矢量os 在矢量 oe的顺时针方向
    return -fi * DPPi;
}

/// <summary>
/// 判断点pt3是否在直线上,需要注意的是，浮点数与0的比较通常为 < 1e-6。
/// </summary>
bool GeometricUtil::isPointInLine(const QPointF &pt1, const QPointF &pt2, const QPointF &pt3)
{
    if ((fabs(pt1.x() - pt2.x()) < 1e-6)) // 判断是否平行于Y轴
    {
        return (fabs(pt1.x() - pt3.x()) < 5);
    }

    // 因为 y = ax + b

    double A = (pt1.y() - pt2.y()) / (pt1.x() - pt2.x());
    double B = (pt1.y() - A * pt1.x());

    return (fabs(A * pt3.x() - pt3.y() + B) < 5);
}

/// <summary>
/// 判断点point是否在多边形内。不需要最后一个点与第一个点相同
/// </summary>
bool GeometricUtil::isPointInPoly(int nvert, QPointF *poly, const QPointF &point, bool judgeInMaxMin = true)
{
    if (nvert < 1)
        return false;

    if (judgeInMaxMin)
    {
        int minX = poly[0].x();
        int maxX = poly[0].x();
        int minY = poly[0].y();
        int maxY = poly[0].y();
        for (int i = 1; i < nvert; i++)
        {
            if (poly[i].x() < minX)
                minX = poly[i].x();
            else if (poly[i].x() > maxX)
                maxX = poly[i].x();
            if (poly[i].y() < minY)
                minY = poly[i].y();
            else if (poly[i].y() > maxY)
                maxY = poly[i].y();
        }
        if (point.x() < minX || point.x() > maxX || point.y() < minY || point.y() > maxY)
            return false;
    }

    int i, j, c = 0;
    for (i = 0, j = nvert - 1; i < nvert; j = i++)
    {
        if (((poly[i].y() > point.y()) != (poly[j].y() > point.y())) &&
            (point.x() < (poly[j].x() - poly[i].x()) * (point.y() - poly[i].y()) / (poly[j].y() - poly[i].y()) + poly[i].x()))
            c = !c;
    }
    return c;
}

bool GeometricUtil::isPointInPoly(const QVector<QPointF> &poly, const QPointF &point, bool judgeInMaxMin)
{
    int nvert = poly.count();
    if (nvert < 3)
        return false;

    if (judgeInMaxMin)
    {
        int minX = poly[0].x();
        int maxX = poly[0].x();
        int minY = poly[0].y();
        int maxY = poly[0].y();
        for (int i = 1; i < nvert; i++)
        {
            if (poly[i].x() < minX)
                minX = poly[i].x();
            else if (poly[i].x() > maxX)
                maxX = poly[i].x();
            if (poly[i].y() < minY)
                minY = poly[i].y();
            else if (poly[i].y() > maxY)
                maxY = poly[i].y();
        }
        if (point.x() < minX || point.x() > maxX || point.y() < minY || point.y() > maxY)
            return false;
    }

    int i, j, c = 0;
    for (i = 0, j = nvert - 1; i < nvert; j = i++)
    {
        if (((poly[i].y() > point.y()) != (poly[j].y() > point.y())) &&
            (point.x() < (poly[j].x() - poly[i].x()) * (point.y() - poly[i].y()) / (poly[j].y() - poly[i].y()) + poly[i].x()))
            c = !c;
    }
    return c;
}

///// <summary>
///// 获取两个点之间的距离
///// </summary>
double GeometricUtil::getDistance(const QPointF &pt1, const QPointF &pt2)
{
    return sqrt((pt1.x() - pt2.x()) * (pt1.x() - pt2.x()) + (pt1.y() - pt2.y()) * (pt1.y() - pt2.y()));
}

double GeometricUtil::getDistance(QPoint pt1, QPoint pt2)
{
    return sqrt((pt1.x() - pt2.x()) * (pt1.x() - pt2.x()) + (pt1.y() - pt2.y()) * (pt1.y() - pt2.y()));
}

double GeometricUtil::getDistance(double x1, double y1, double x2, double y2)
{
    return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

///// <summary>
///// 获取点s到两个点构成的直线的距离
///// </summary>
double GeometricUtil::getPtToLineDistance(const QPointF &a, const QPointF &b, const QPointF &s)
{
    double x1 = a.x();
    double y1 = a.y();
    double x2 = b.x();
    double y2 = b.y();

    double dx = x2 - x1;
    double dy = y2 - y1;

    double cross = dx * (s.x() - x1) + dy * (s.y() - y1);
    if (cross <= 0)
        return qSqrt((s.x() - x1) * (s.x() - x1) + (s.y() - y1) * (s.y() - y1));

    double d2 = dx * dx + dy * dy;
    if (cross >= d2)
        return qSqrt((s.x() - x2) * (s.x() - x2) + (s.y() - y2) * (s.y() - y2));

    double r = cross / d2;
    double px = x1 + dx * r;
    double py = y1 + dy * r;
    return qSqrt((s.x() - px) * (s.x() - px) + (py - s.y()) * (py - s.y()));
}

double GeometricUtil::getPtToLineDistance(double x1, double y1, double x2, double y2, const QPointF &s)
{
    return getPtToLineDistance(QPointF(x1, y1), QPointF(x2, y2), s);
}

///// <summary>
///// 判断点(x,y)是否在椭圆上。椭圆在两点构成的矩形内。0：在线上；-1：内；1：外
///// </summary>
int GeometricUtil::isPtInEllipse(double x1, double y1, double x2, double y2, double x, double y)
{
    double A = fabs(x2 - x1) / 2.0;
    double B = fabs(y2 - y1) / 2.0;
    double cx = (x1 + x2) / 2.0;
    double cy = (y1 + y2) / 2.0;
    cx -= x;
    cy -= y;
    double dis = pow(cx / A, 2) + pow(cy / B, 2);
    if (dis > 1.001)
        return 1;
    else if (dis < 0.99)
        return -1;
    else
        return 0;
}

double GeometricUtil::dist2d(const QPointF &pt1, const QPointF &pt2)
{
    double dx = pt1.x() - pt2.x();
    double dy = pt1.y() - pt2.y();
    return qSqrt(dx * dx + dy * dy);
}

bool GeometricUtil::equals(const QPointF &pt1, const QPointF &pt2)
{
    const double disErr = 1;
    return (qAbs(pt1.x() - pt2.x()) <= disErr && qAbs(pt1.y() - pt2.y()) < disErr);
}

void GeometricUtil::parallelLine(QVector<QPointF> &line, double offset, bool isLeft, QVector<QPointF> &path)
{
    int n = line.size();
    if (n < 2)
        return;

    if (CommonUtil::floatEqual(offset, 0.0))
    {
        path.append(line);
        return;
    }
    for (int i = 0; i < n; i += 2)
    {
        QPointF outPt1, outPt2;
        parallelLine(line.at(i), line.at(i + 1), offset, isLeft, outPt1, outPt2);
        path.append(outPt1);
        path.append(outPt2);
    }

    if (n % 2 == 1)
    {
        QPointF outPt1, outPt2;
        parallelLine(line[n - 2], line[n - 1], offset, isLeft, outPt1, outPt2);
        path.append(outPt2);
    }

#if 0
    int N = line.length() - 1;
    int max = N;
    double mi, mi1, li, li1, ri, ri1, si, si1, Xi1, Yi1;
    QPointF p0, p1, p2;
    bool isClosed = equals(line.first(), line.last());
    if (!isClosed)
    {
        p0 = line[0];
        p1 = line[1];
        double dist = GeometricUtil::dist2d(p0, p1);
        p2.setX(p0.x() + (p1.y() - p0.y()) / dist * offset);
        p2.setY(p0.y() - (p1.x() - p0.x()) / dist * offset);

        path.push_back(p2);
        line.push_back(line[N]);
        N++;
        max--;
    }

    for (int i = 0; i < max; i++)
    {
        p0 = line[i];
        p1 = line[(i + 1) % N];
        p2 = line[(i + 2) % N];
        mi = (p1.y() - p0.y()) / (p1.x() - p0.x());
        mi1 = (p2.y() - p1.y()) / (p2.x() - p1.x());
        if (qAbs(mi - mi1) <= 1e-10)
            continue;

        li = GeometricUtil::dist2d(p0, p1);
        li1 = GeometricUtil::dist2d(p1, p2);

        ri = p0.x() + offset * (p1.y() - p0.y()) / li;
        ri1 = p1.x() + offset * (p2.y() - p1.y()) / li1;

        si = p0.y() - offset * (p1.x() - p0.x()) / li;
        si1 = p1.y() - offset * (p2.x() - p1.x()) / li1;

        Xi1 = (mi1 * ri1 - mi * ri + si - si1) / (mi1 - mi);
        Yi1 = (mi * mi1 * (ri1 - ri) + mi1 * si - mi * si1) / (mi1 - mi);

        if (qAbs(p1.x() - p0.x()) <= 1e-10)
        {
            Xi1 = p1.x() + offset * (p1.y() - p0.y()) / qAbs(p1.y() - p0.y());
            Yi1 = mi1 * Xi1 - mi1 * ri1 + si1;
        }
        if (qAbs(p2.x() - p1.x()) <= 1e-10)
        {
            Xi1 = p2.x() + offset * (p2.y() - p1.y()) / qAbs(p2.y() - p1.y());
            Yi1 = mi * Xi1 - mi * ri + si;
        }
        path.push_back(QPointF(Xi1, Yi1));
    }

    if (isClosed)
        path.push_back(path.first());
    else
    {
        line.pop_back();
        p0 = line[line.length() - 1];
        p1 = line[line.length() - 2];

        double dist = GeometricUtil::dist2d(p0, p1);
        p2.setX(p0.x() - (p1.y() - p0.y()) / dist * offset);
        p2.setY(p0.y() + (p1.x() - p0.x()) / dist * offset);
        path.push_back(p2);
    }
#endif
}

void GeometricUtil::parallelLine(const QPointF &pt1, const QPointF &pt2, double offset, bool isLeft, QPointF &outPt1, QPointF &outPt2)
{
    double bearing = Mercator::calculateBearingXY(pt1.x(), pt1.y(), pt2.x(), pt2.y());
    double offsetBearing = bearing - (isLeft ? 90 : -90);

    // 墨卡托投影的坐标的y轴向上，和常规的窗口坐标系统相反；上面取得的角度是和正北，也就是Y轴形成的角度
    outPt1.setX(pt1.x() + offset * sin(qDegreesToRadians(offsetBearing)));
    outPt1.setY(pt1.y() + offset * cos(qDegreesToRadians(offsetBearing)));

    outPt2.setX(pt2.x() + offset * sin(qDegreesToRadians(offsetBearing)));
    outPt2.setY(pt2.y() + offset * cos(qDegreesToRadians(offsetBearing)));
}

bool GeometricUtil::intersectsOfPolyPoly(const QVector<QPointF> &poly1, const QVector<QPointF> &poly2)
{
    bg::model::polygon<BPointF> b_poly1;
    int n = poly1.count();
    for (int i = 0; i < n; i++)
    {
        BPointF p(poly1.at(i).x(), poly1.at(i).y());
        b_poly1.outer().push_back(p);
    }

    n = poly2.count();
    bg::model::polygon<BPointF> b_poly2;
    for (int i = 0; i < n; i++)
    {
        BPointF p(poly2.at(i).x(), poly2.at(i).y());
        b_poly2.outer().push_back(p);
    }
    return bg::intersects(b_poly1, b_poly2);
}

bool GeometricUtil::intersectsOfPolyLine(const QVector<QPointF> &poly, const QVector<QPointF> &line)
{
    bg::model::polygon<BPointF> b_poly;
    int n = poly.count();
    for (int i = 0; i < n; i++)
    {
        BPointF p(poly.at(i).x(), poly.at(i).y());
        b_poly.outer().push_back(p);
    }

    bg::model::linestring<BPointF> b_line;
    n = line.count();
    for (int i = 0; i < n; i++)
    {
        BPointF p(line.at(i).x(), line.at(i).y());
        b_line.push_back(p);
    }

    return bg::intersects(b_poly, b_line);
}
